
export function NormalizeTheta(theta, lastTheta) {
    theta = wrap(theta, -Math.PI, Math.PI);
    const diff = Math.abs(theta - lastTheta);
    if (Math.abs(theta - (Math.PI * 2) - lastTheta) < diff) theta -= (Math.PI * 2);
    if (Math.abs(theta + (Math.PI * 2) - lastTheta) < diff) theta += (Math.PI * 2);
    return theta;
}

function wrap(n, min, max) {
    const d = max - min;
    const w = ((n - min) % d + d) % d + min;
    return (w === min) ? max : w;
}